#include <cmath>

#include "Spring.h"

namespace BlobSalladDS
{
	Spring::Spring(double restLength, double stiffness, double damper, PointMass *pointMassA, PointMass *pointMassB)
	{
		this->restLength = restLength;
		this->stiffness = stiffness;
		this->damper = damper;
		this->pointMassA = pointMassA;
		this->pointMassB = pointMassB;
		this->tmp = new Vector(0.0, 0.0);
	}

	void Spring::sc(Environment &env)
	{
		env.collision(*(this->pointMassA->getPos()), *(this->pointMassA->getPrevPos()));
		env.collision(*(this->pointMassB->getPos()), *(this->pointMassB->getPrevPos()));
	}

	void Spring::move(double dt)
	{
		/*
		double aXbX;
		double aYbY;
		double springForce;
		double length;

		aXbX = this->pointMassA->getXPos() - this->pointMassB->getXPos();
		aYbY = this->pointMassA->getYPos() - this->pointMassB->getYPos();

		length = sqrt(aXbX * aXbX + aYbY * aYbY);
		springForce = this->stiffness * (length / this->restLength - 1.0);

		double avXbvX;
		double avYbvY;
		double damperForce;

		avXbvX = this->pointMassA->getXVel() - this->pointMassB->getXVel();
		avYbvY = this->pointMassA->getYVel() - this->pointMassB->getYVel();

		damperForce = avXbvX * aXbX + avYbvY * aYbY;
		damperForce *= this->damper;

		double fx;
		double fy;

		fx = (springForce + damperForce) * aXbX;
		fy = (springForce + damperForce) * aYbY;

		this->tmp->setX(-fx);
		this->tmp->setY(-fy);
		this->pointMassA->addForce(*(this->tmp));

		this->tmp->setX(fx);
		this->tmp->setY(fy);
		this->pointMassB->addForce(*(this->tmp));

		this->pointMassA->move(dt);
		this->pointMassB->move(dt);
		*/
	}

	void Spring::addForce(Vector *force)
	{
		this->pointMassA->addForce(*force);
		this->pointMassB->addForce(*force);
	}

	void Spring::draw(void * ctx, double scaleFactor)
	{
		//TODO: draw using PAlib
	}

}
